//
// Created by bit on 2022/8/1.
//

#ifndef UNTITLED3_POSFILTER_H
#define UNTITLED3_POSFILTER_H

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <iostream>
#include <sys/time.h>
#include <fstream>
#include "get_time.h"

using namespace Eigen;

class PosKalmanFilter {
public:
    PosKalmanFilter(float dt);
    ~PosKalmanFilter() = default;

    bool update(float N_input, float E_input, float &N, float &E, float &v_N, float &v_E);

    void setInitState(float N, float E, float v_n, float v_e);

    void setInput(float input) {u_input_ = input;}

    void setMeansUncertainty(float value);

    float getMeansUncertainty() {return Means_uncertainty_;}

private:
    void init();

    float angle_lim(float value, float lim) {
        if (value > lim) {
            value -= 360.0f;
            return angle_lim(value, lim);
        } else if (value < -lim) {
            value += 360.0f;
            return angle_lim(value, lim);
        } else {
            return value;
        }
    }

private:
    uint8_t init_phase_{0};
    std::ofstream output_data;

    float N_init_, E_init_;
    uint64_t time_start_{0};
    uint64_t time_init_{0};
    uint64_t time_loop_{0};

    float dt_{0.1f};
    float u_input_{0.0f};

    Matrix<float, 4,4> A_m_;        // 状态转移矩阵
    Matrix<float, 4,1> B_m_;        // 状态控制矩阵
    Matrix<float, 2,4> H_m_;        // 状态到测量的转换矩阵
    Matrix<float, 4,4> S_m_;        // 状态协方差
    Matrix<float, 4,4> Q_m_;        // 过程激励噪声协方差
    Matrix<float, 2,2> R_m_;        // 测量噪声矩阵

    float state_uncertainty_{5.0f};
    float Means_uncertainty_{1.0f};   // 控制测量误差，与Rm有关

    Matrix<float, 4,1> x_m_;        // 状态量
    Matrix<float, 2,1> input_;      // 滤波器的输入量，即x1、x2
    Matrix<float, 4,2> K_m_;        // 卡尔曼增益
    Matrix<float, 4,4> eye4_;       // 四维单位矩阵
};




#endif //UNTITLED3_POSFILTER_H
